/*
attitude.pde

Author:     Uwe Gartmann

This firmware is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

****************************************************************************************************
AHRSupdate by S.O.H. Madgwick, 25th August 2010

User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
Global variables 'q[0]', 'q[1]', 'q[2]', 'q[3]' are the quaternion elements representing the estimated
orientation.  See my report for an overview of the use of quaternions in this application.
User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
****************************************************************************************************
*/

// Axis definition:
// X axis pointing forward
// Y axis pointing to the right
// and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise
// theta = The pitch in degrees relative to a plane(mathematical) normal to the earth radius line at the point the plane(aircraft) is at
// phi = The roll of the aircraft in degrees
// psi = The true heading of the aircraft in degrees

/// Definitions
#define bit_set(p,m)   ((p) |= ( 1<<m))
#define bit_clear(p,m) ((p) &= ~(1<<m))

#define Kp 2.0f			// proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.2f			// integral gain governs rate of convergence of gyroscope biases
#define halfT 0.025f	// half the sample period

/// Declarations

// quaternion elements representing the estimated orientation
float q[4] = { 1.0, 0.0, 0.0, 0.0 };

// euler for angles in orientations
float euler[4];

// scaled integral error
float exInt = 0.0;
float eyInt = 0.0;
float ezInt = 0.0;		

void updateAHRS(void) {
	//bit_set(PORTF,6);
	AHRSupdate( ToRad(IMU.Sensor[IMU_roll]),
				ToRad(IMU.Sensor[IMU_pitch]),
				ToRad(IMU.Sensor[IMU_yaw]),
				IMU.Sensor[IMU_accX],
				IMU.Sensor[IMU_accY],
				IMU.Sensor[IMU_accZ],
				Compass.Mag_Xrel,
				Compass.Mag_Yrel,
				Compass.Mag_Zrel);
	
	/*
	IMUupdate(	ToRad(IMU.Sensor[IMU_roll]),
				ToRad(IMU.Sensor[IMU_pitch]),
				ToRad(IMU.Sensor[IMU_yaw]),
				IMU.Sensor[IMU_accX],
				IMU.Sensor[IMU_accY],
				IMU.Sensor[IMU_accZ]);
	*/
	// Send quaternions to serial for 3D view
	serialFloatPrint(q[0]);
	Serial0.print(",");
	serialFloatPrint(q[1]);
	Serial0.print(",");
	serialFloatPrint(q[2]);
	Serial0.print(",");
	serialFloatPrint(q[3]);
	Serial0.print("\n");
	
	//bit_clear(PORTF,6);
	quaternionToEuler();
}

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float norm;
	float hx, hy, hz, bx, bz;
	float vx, vy, vz, wx, wy, wz;
	float ex, ey, ez;

	// auxiliary variables to reduce number of repeated operations
	float q0q0 = q[0]*q[0];
	float q0q1 = q[0]*q[1];
	float q0q2 = q[0]*q[2];
	float q0q3 = q[0]*q[3];
	float q1q1 = q[1]*q[1];
	float q1q2 = q[1]*q[2];
	float q1q3 = q[1]*q[3];
	float q2q2 = q[2]*q[2];   
	float q2q3 = q[2]*q[3];
	float q3q3 = q[3]*q[3];          
	
	// normalise the measurements
	norm = sqrt(ax*ax + ay*ay + az*az);       
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;
	norm = sqrt(mx*mx + my*my + mz*mz);          
	mx = mx / norm;
	my = my / norm;
	mz = mz / norm;         
	
	// compute reference direction of flux
	hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
	hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
	hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
	bx = sqrt((hx*hx) + (hy*hy));
	bz = hz;        
	
	// estimated direction of gravity and flux (v and w)
	vx = 2*(q1q3 - q0q2);
	vy = 2*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;
	wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
	wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
	wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
	
	// error is sum of cross product between reference direction of fields and direction measured by sensors
	ex = (ay*vz - az*vy) + (my*wz - mz*wy);
	ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
	ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
	
	// integral error scaled integral gain
	exInt = exInt + ex*Ki;
	eyInt = eyInt + ey*Ki;
	ezInt = ezInt + ez*Ki;
	
	// adjusted gyroscope measurements
	gx = gx + Kp*ex + exInt;
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;
	
	// integrate quaternion rate and normalise
	q[0] = q[0] + (-q[1]*gx - q[2]*gy - q[3]*gz)*halfT;
	q[1] = q[1] + (q[0]*gx + q[2]*gz - q[3]*gy)*halfT;
	q[2] = q[2] + (q[0]*gy - q[1]*gz + q[3]*gx)*halfT;
	q[3] = q[3] + (q[0]*gz + q[1]*gy - q[2]*gx)*halfT;  
	
	// normalise quaternion
	norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	q[0] = q[0] / norm;
	q[1] = q[1] / norm;
	q[2] = q[2] / norm;
	q[3] = q[3] / norm;
}

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
	float norm;
	float vx, vy, vz;
	float ex, ey, ez;         
	
	// normalise the measurements
	norm = sqrt(ax*ax + ay*ay + az*az);       
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;      
	
	// estimated direction of gravity
	vx = 2*(q[1]*q[3] - q[0]*q[2]);
	vy = 2*(q[0]*q[1] + q[2]*q[3]);
	vz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
	
	// error is sum of cross product between reference direction of field and direction measured by sensor
	ex = (ay*vz - az*vy);
	ey = (az*vx - ax*vz);
	ez = (ax*vy - ay*vx);
	
	// integral error scaled integral gain
	exInt = exInt + ex*Ki;
	eyInt = eyInt + ey*Ki;
	ezInt = ezInt + ez*Ki;
	
	// adjusted gyroscope measurements
	gx = gx + Kp*ex + exInt;
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;
	
	// integrate quaternion rate and normalise
	q[0] = q[0] + (-q[1]*gx - q[2]*gy - q[3]*gz)*halfT;
	q[1] = q[1] + (q[0]*gx + q[2]*gz - q[3]*gy)*halfT;
	q[2] = q[2] + (q[0]*gy - q[1]*gz + q[3]*gx)*halfT;
	q[3] = q[3] + (q[0]*gz + q[1]*gy - q[2]*gx)*halfT;  
	
	// normalise quaternion
	norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
	q[0] = q[0] / norm;
	q[1] = q[1] / norm;
	q[2] = q[2] / norm;
	q[3] = q[3] / norm;
}

void quaternionToEuler(void) {
	// Heading >> psi
	euler[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
	
	// Pitch >> theta
	euler[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]);
	
	// Roll >> phi
	euler[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1);
}

void EulerToAttitude(void) {

}